/*******************************************************************
*
*    FILE: robot_interrupts.c
*    Description: 
*    AUTHOR: Tim Cook and Nate Lannan
*    DATE:9/20/2009
*
*******************************************************************/

/** include files **/
#include <p18f4520.h>
#include <adc.h>
#include "ieee_robot.h"
#include "servo.h"
#include "motor.h"

void InterruptHandlerHigh ();
void InterruptHandlerLow ();

/** external data **/
extern volatile unsigned short int servo_position;
extern volatile unsigned short int SLOW_MOTOR_FLAG;
extern volatile unsigned short int a2d_result; 
extern volatile unsigned short int reverse_flag;
extern volatile unsigned short int millisecond_count;
extern volatile unsigned char portb_int;
extern short pos;
extern unsigned char servo_command[CMD_SIZE];

/** public functions **/
#pragma code InterruptVectorHigh = 0x08
void InterruptVectorHigh (void) {
    _asm
        goto InterruptHandlerHigh //jump to interrupt routine
    _endasm
}

#pragma code InterruptVectorLow = 0x18
void InterruptVectorLow(void){
	_asm
		goto InterruptHandlerLow
	_endasm
}

#pragma code
#pragma interrupt InterruptHandlerHigh
void InterruptHandlerHigh () {
#if SERVO == CONTROLLER
     if (PIR1bits.TXIF) {
        if (pos < 2) {
            TXREG = servo_command[++pos];
        } else {
            pos = -1;
            PIE1 = PIE1 & (~0x10);
        }
    }
#elif SERVO == TIMERS
    if (TMR0_INT_FLAG) {            //check for TMR0 overflow
        INTCONbits.TMR0IF = 0;      //clear interrupt flag for TMR1
        write_timer0( TMR0_VALUE );   //write the value to TMR0
            //required to generate 50 Hz interrrupts that is defined
            //in servo.h

        //write direction to turn and
        //enable TMR1 interrupts and then turn on TMR1
        write_timer1( servo_position );
        PIE1bits.TMR1IE = 1;
        T1CONbits.TMR1ON = 1;
         
        //write to servo
        PORTCbits.RC3 = 1;     //write a 1 to servo port

        //enable TMR1 peripheral
        //T1CON = T1CON | 0x01; 
        
    } else if ( TMR1_INT_FLAG ){
        PIR1 = PIR1 & 0xFE; //clear TMR1 interrupt flag
        PIE1 = PIE1 & 0xFE; //disable TMR1 interrupts
        T1CONbits.TMR1ON = 0;//disable T1
        //clear servo pin for low part of cycle
        PORTCbits.RC3 = 0;
    }
#else
    #warning "Must have method of controlling servo!!!!, robot_interrupts.c"
#endif
    //INTCONbits.GIEH = 1; //renable global interrupts
    //INTCONbits.PEIE = 1; //renable peripheral interrups
    enable_int_all(); //enable all interrupts 
}


#pragma code
#pragma interrupt InterruptHandlerLow
void InterruptHandlerLow() {
//====================formula for A/D============================================
//
//  (Vcc-Vref)/1024 = volts per step
//  in case of 5 volt rail:
//  (5 - 0)/1024 = 4.88 mV per step
//  therefore, value in a2d_result*0.00488 = actual voltage
//=============================================================================== 
      if ( PIR2bits.TMR3IF == 1 ) {/* timer 3 interrupts*/
          ++millisecond_count; /* increment millisecond count */
          PIR2 = PIR2 & 0xFD;  /* clear TMR3IF */
          tmr3_reset(); /* reset millisecond value to timer 3*/
          PIE2 = PIE2 | 0xFD;  /* re-enable tmr3 interrupts*/
     } else if (PIR1bits.ADIF == 1){		//ADC caused interrupt
		PIE1bits.ADIE = 0;
          ADCON0bits.GO = 0;	//turn off GO bit
          a2d_result = ReadADC();	//grab conversion
          //PORTB = a2d_result;	/for debugging
          PIR1bits.ADIF = 0;		//turn off interrupt flag
          PIE1bits.ADIE = 1;	     //enable ADC interrupts
     	if (a2d_result < VCC_THRESH){
     		ConvertADC();	 //start next conversion
     	}
	} else if( INTCONbits.RBIF == 1 ){ //whisker interrupts
          set_servo_position();
          #if DEBUG == 1
               PORTBbits.RB3 = INTCON3bits.INT2F;			//debug purpose
               PORTBbits.RB0 = INTCONbits.RBIF;
          #endif
          INTCONbits.RBIF = 0;    //clear PORTB interrupt flag
          enable_wisker_interrupts(); 
     } 

    enable_int_all(); /* re-enable all interrupts*/
}

#pragma interrupt InterruptHandlerLow save=WREG


